/*
    This source file is part of GeNa project.
    Copyright © GeNa project 2008
    For the latest info, see http://code.google.com/p/genaproject/ or  http://lumpyproject.forums-rpg.com

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Foobar is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Foobar.  If not, see <http://www.gnu.org/licenses/>. */

#ifndef FIXED_H_INCLUDED
#define FIXED_H_INCLUDED

#include <cmath>
#include <iostream>

#include <boost/operators.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>
using namespace boost;

#include "math/misc.h" //For De Bruign sequences

namespace math
{

class angle;

template<int n> struct Mask
{
    enum
    {
        val = (Mask<n-1>::val << 1) + 1l
    };
};

template<> struct Mask<0>
{
    enum
    {
        val = 0
    };
};

//Work with 32 and 64 bits, T should be either “long” or “long long”
template<class T, int mantissaBitCount> class FixedPoint :  less_than_comparable< FixedPoint<T, mantissaBitCount>,
                                                            equality_comparable< FixedPoint<T, mantissaBitCount>,
                                                            addable< FixedPoint<T, mantissaBitCount>,
                                                            subtractable< FixedPoint<T, mantissaBitCount>,
                                                            multipliable< FixedPoint<T, mantissaBitCount>,
                                                            dividable< FixedPoint<T, mantissaBitCount>
                                                            > > > > > >
{
public:
    enum
    {
        bitCount = 8*sizeof(T),
        signeBit = bitCount -1,
        valBitCount = bitCount - 1,
        mask = Mask<bitCount>::val, //Mask on all T bits
        valMask = Mask<bitCount-1>::val, //Mask on all T bits except sign bit
        integerMask = Mask<valBitCount-mantissaBitCount>::val << mantissaBitCount, //Mask on integer part
        mantissaMask = Mask<mantissaBitCount>::val
    };

    T n;

    FixedPoint()
    {
    }

    FixedPoint & operator=(const FixedPoint &_n)
    {
        n = _n.n;
        return *this;
    }

    //Conversion to FixedPoint
    template<class U, int m> FixedPoint(const FixedPoint<U,m> &_n)
    {

        if( (int)FixedPoint<U,m>::bitCount > (int)bitCount)
        {
            U n2 = _n.n;

            int a = mantissaBitCount - m;
            if( mantissaBitCount > m)
                n2 <<= a;
            else if( m < mantissaBitCount)
                n2 >>= -a;

            n = n2;
        }
        else
        {
            n = _n.n;

            int a = mantissaBitCount - m;
            if( mantissaBitCount > m)
                n <<= a;
            else if( m < mantissaBitCount)
                n >>= a;
        }
    }

    //To angle
    FixedPoint(angle a);

    //Integer conversions

    FixedPoint(unsigned int _n)
    {
        n = _n << mantissaBitCount;
    }

    FixedPoint(unsigned short int _n)
    {
        n = _n << mantissaBitCount;
    }

    FixedPoint(unsigned long int _n)
    {
        n = _n << mantissaBitCount;
    }

    FixedPoint(unsigned long long int _n)
    {
        n = _n << mantissaBitCount;
    }

    FixedPoint(unsigned char _n)
    {
        n = _n << mantissaBitCount;
    }


    FixedPoint(int _n)
    {
        n = _n << mantissaBitCount;
    }

    FixedPoint(short int _n)
    {
        n = _n << mantissaBitCount;
    }

    FixedPoint(long int _n)
    {
        n = _n << mantissaBitCount;
    }

    FixedPoint(long long int _n)
    {
        n = _n << mantissaBitCount;
    }

    FixedPoint(char _n)
    {
        n = _n << mantissaBitCount;
    }

    //Floating-point conversion
    FixedPoint(float f) //For IEEE-754 Simple precison
    {
        unsigned int _f = reinterpret_cast<int&>(f);

        int e = ((_f >> 23) & Mask<8>::val) - 127;

        n = (reinterpret_cast<T&>(_f) & Mask<23>::val) | (1 << 23);

        bool s = _f >> 31;

        if( e == -127)
            n = 0; //Denormalized number
        else if( e == (2 << 8)-1)
        {
            if( n)
                n = 0; //NaN
            else if( s)
                n = -valMask; // -Infinity
            else
                n = valMask; // +Infinity
        }
        else
        {
            if( mantissaBitCount + e - 23 >= 0)
                n <<= mantissaBitCount + e - 23;
            else
                n >>=  -mantissaBitCount - e + 23;
            n &= valMask;

            if( s)
                n = -n;
        }
    }

    FixedPoint(double f) //For IEEE-754 Double precison
    {
        unsigned long long int _f = reinterpret_cast<unsigned long long int &>(f);

        int e = ((_f >> 52) & Mask<11>::val) - 1023;

        int eatedBits = 53 - valBitCount;
        if( eatedBits < 0)
            eatedBits = 0;

        n = ((_f & 0xfffffffffffffll) | (1ll << 52)) >> eatedBits; //Keep only most significant bits

        bool s = _f >> 63;

        if( e == -1023)
            n = 0; //Denormalized number
        else if( e == (2 << 11)-1)
        {
            if( n)
                n = 0; //NaN
            else if( s)
                n = -valMask; // -Infinity
            else
                n = valMask; // +Infinity
        }
        else
        {
            if( mantissaBitCount + e - 52 + eatedBits >= 0)
                n <<= mantissaBitCount + e - 52 + eatedBits;
            else
                n >>=  -mantissaBitCount - e + 52 - eatedBits;

            if( s)
                n = -n;
        }
    }


    //Conversion from Fixed point
    template<class U> U toUnsignedInteger() const
    {
        T t = (n & valMask) >> mantissaBitCount;
        return reinterpret_cast<U&>(t);
    }

    template<class U> U toSignedInteger() const
    {
        T t = n >> mantissaBitCount;
        U u = reinterpret_cast<U&>(t);
        return u;
    }

    operator unsigned int () const
    {
        return toUnsignedInteger<unsigned int>();
    }

    operator unsigned short int () const
    {
        return toUnsignedInteger<unsigned short int>();
    }

    operator unsigned long int () const
    {
        return toUnsignedInteger<unsigned long int>();
    }

    operator unsigned long long int () const
    {
        return toUnsignedInteger<unsigned long long int>();
    }

    operator unsigned char () const
    {
        return toUnsignedInteger<unsigned char>();
    }

    operator int () const
    {
        return toSignedInteger<int>();
    }

    operator short int () const
    {
        return toSignedInteger<short int>();
    }

    operator long int () const
    {
        return toSignedInteger<long int>();
    }

    operator long long int () const
    {
        return toSignedInteger<long long int>();
    }

    operator char () const
    {
        return toSignedInteger<char>();
    }

    operator float () const
    {
        if( n == 0)
            return 0;

        int mask = n >> signeBit;
        T v = (n + mask) ^ mask;
        int pos;

        //Find last non-zero digit
        if( bitCount == 32)
        {

            v |= v >> 1; // first round down to power of 2
            v |= v >> 2;
            v |= v >> 4;
            v |= v >> 8;
            v |= v >> 16;
            v = (v >> 1) + 1;

            //Use De Bruign sequence
            pos = DeBruign5LookupTable[((unsigned long int)v * DeBruign5Seq) >> 27];
        }
        else
        {

            //Find last non-zero digit
            v |= v >> 1; // first round down to power of 2
            v |= v >> 2;
            v |= v >> 4;
            v |= v >> 8;
            v |= v >> 16;
            int i = 32; //Avoid warning
            v |= v >> i;
            v = (v >> 1) + 1;

            //Use De Bruign sequence
            pos = DeBruign6LookupTable[((unsigned long long int)v * DeBruign6Seq) >> 54];
        }

        unsigned int _n = (n + mask) ^ mask;
        if( pos > 23)
            _n >>= pos - 23;
        else
            _n <<= 23 - pos;
        _n &= Mask<23>::val;

        //Find exponent
        int e = pos - mantissaBitCount;

        int s = n < 0;

        unsigned int _f = (s << 31) | ((e+127)<<23) | _n;
        return reinterpret_cast<float&>(_f);
    }

    operator double () const
    {
        if( n == 0)
            return 0;

        int mask = n >> signeBit;
        T v = (n + mask) ^ mask;
        int pos;

        //Find last non-zero digit
        if( bitCount == 32)
        {

            v |= v >> 1; // first round down to power of 2
            v |= v >> 2;
            v |= v >> 4;
            v |= v >> 8;
            v |= v >> 16;
            v = (v >> 1) + 1;

            //Use De Bruign sequence
            pos = DeBruign5LookupTable[((unsigned long int)v * DeBruign5Seq) >> 27];
        }
        else
        {
            T v = (n + valMask) ^ valMask;

            //Find last non-zero digit
            v |= v >> 1; // first round down to power of 2
            v |= v >> 2;
            v |= v >> 4;
            v |= v >> 8;
            v |= v >> 16;
            int i = 32; //Avoid warning
            v |= v >> i;
            v = (v >> 1) + 1;

            //Use De Bruign sequence
            pos = DeBruign6LookupTable[((unsigned long long int)v * DeBruign6Seq) >> 54];
        }

        unsigned long long int _n = (n + mask) ^ mask;

        if( pos > 52)
            _n >>= pos - 52;
        else
            _n <<= 52 - pos;
        _n &= 0xfffffffffffffll;

        //Find exponent
        int e = pos - mantissaBitCount;

        int s = n < 0;

        unsigned long long int _f = ((unsigned long long int)s << 63) | ((unsigned long long int)(e+1023)<<52) | _n;
        return reinterpret_cast<double&>(_f);
    }

    //Operators


    bool operator<( const FixedPoint &_n) const
    {
        return n < _n.n;
    }

    bool operator==( const FixedPoint &_n) const
    {
        return n == _n.n;
    }

    FixedPoint & operator+=(const FixedPoint &_n)
    {
        n += _n.n;
        return *this;
    }

    FixedPoint operator-()
    {
        FixedPoint f;
        f.n = -n;
        return f;
    }

    FixedPoint & operator-=(const FixedPoint &_n)
    {
        n -= _n.n;
        return *this;
    }

    FixedPoint & operator*=(const FixedPoint &_n)
    {
        signed long long int n1 = (signed long long)n * (signed long long)_n.n;
        n = n1 >> mantissaBitCount;
        return *this;
    }

    FixedPoint & operator/=(const FixedPoint &_n)
    {
        if( _n.n == 0)
        {
            if( n < 0)
                n = -valMask; // -infinity
            else if( n > 0)
                n = valMask; // + infinity
            else
                n = 0; // error ?
            return *this;
        }

        signed long long int n2 = n;
        n2 = (n2 << mantissaBitCount) / (signed long long)_n.n;
        n = n2;

        return *this;
    }

    FixedPoint abs()
    {
        int mask = n >> signeBit;
        return FixedPoint( (n + mask) ^ mask);
    }

    template<class Archive> void serialize(Archive & ar, const unsigned int version);
};

class angle :   less_than_comparable<angle,
                equality_comparable< angle,
                addable< angle,
                subtractable< angle,
                multipliable< angle,
                dividable< angle
                > > > > > >
{
public:
    enum
    {
        mantissaBitCount = 8*sizeof(int)-1,
        bitCount = 8*sizeof(int),
        signeBit = bitCount -1,
        valBitCount = bitCount - 1,
        mask = Mask<bitCount>::val, //Mask on all bits
        valMask = Mask<valBitCount>::val, //Mask on all bits except sign bit
        integerMask = Mask<valBitCount-mantissaBitCount>::val << mantissaBitCount, //Mask on integer part
        mantissaMask = Mask<mantissaBitCount>::val
    };

    int n;

    angle()
    {
    }

    angle & operator=(const angle &_n)
    {
        n = _n.n;
        return *this;
    }

    //Conversion from FixedPoint
    template<class U, int m> angle(const FixedPoint<U,m> &_n)
    {
        if( (_n == 1) || (_n == -1))
        {
            int b = 0x7fffffff;
            n = b+1;
            return;
        }

        if( (int)FixedPoint<U,m>::bitCount > (int)bitCount)
        {
            U n2 = _n.n;

            int a = mantissaBitCount - m;
            if( mantissaBitCount > m)
                n2 <<= a;
            else if( m < mantissaBitCount)
                n2 >>= -a;

            n = n2;
        }
        else
        {
            n = _n.n;

            int a = mantissaBitCount - m;
            if( mantissaBitCount > m)
                n <<= a;
            else if( m < mantissaBitCount)
                n >>= a;
        }
    }


    //Integer conversions

    angle(unsigned int _n)
    {
        if( n % 2)
        {
            int b = 0x7fffffff;
            n = b+1;
        }
        else
            n = 0;
    }

    angle(unsigned short int _n)
    {
        if( n % 2)
        {
            int b = 0x7fffffff;
            n = b+1;
        }
        else
            n = 0;
    }

    angle(unsigned long int _n)
    {
        if( n % 2)
        {
            int b = 0x7fffffff;
            n = b+1;
        }
        else
            n = 0;
    }

    angle(unsigned long long int _n)
    {
        if( n % 2)
        {
            int b = 0x7fffffff;
            n = b+1;
        }
        else
            n = 0;
    }

    angle(unsigned char _n)
    {
        if( n % 2)
        {
            int b = 0x7fffffff;
            n = b+1;
        }
        else
            n = 0;
    }


    angle(int _n)
    {
        if( _n % 2)
        {
            int b = 0x7fffffff;
            n = b+1;
        }
        else
            n = 0;
    }

    angle(short int _n)
    {
        if( n % 2)
        {
            int b = 0x7fffffff;
            n = b+1;
        }
        else
            n = 0;
    }

    angle(long int _n)
    {
        if( n % 2)
        {
            int b = 0x7fffffff;
            n = b+1;
        }
        else
            n = 0;
    }

    angle(long long int _n)
    {
        if( n % 2)
        {
            int b = 0x7fffffff;
            n = b+1;
        }
        else
            n = 0;
    }

    angle(char _n)
    {
        if( n % 2)
        {
            int b = 0x7fffffff;
            n = b+1;
        }
        else
            n = 0;
    }

    //Floating-point conversion
    angle(float f) //For IEEE-754 Simple precison
    {
        if( (f == 1.0) || (f == -1.0))
        {
            int b = 0x7fffffff;
            n = b+1;
            return;
        }

        unsigned int _f = reinterpret_cast<int&>(f);

        int e = ((_f >> 23) & Mask<8>::val) - 127;

        n = (reinterpret_cast<int&>(_f) & Mask<23>::val) | (1 << 23);

        bool s = _f >> 31;

        if( e == -127)
            n = 0; //Denormalized number
        else if( e == (2 << 8)-1)
        {
            if( n)
                n = 0; //NaN
            else if( s)
                n = -valMask; // -Infinity
            else
                n = valMask; // +Infinity
        }
        else
        {
            if( mantissaBitCount + e - 23 >= 0)
                n <<= mantissaBitCount + e - 23;
            else
                n >>=  -mantissaBitCount - e + 23;
            n &= valMask;

            if( s)
                n = -n;
        }
    }

    angle(double f) //For IEEE-754 Double precison
    {
        if( (f == 1.0) || (f == -1.0))
        {
            int b = 0x7fffffff;
            n = b+1;
            return;
        }

        unsigned long long int _f = reinterpret_cast<unsigned long long int &>(f);

        int e = ((_f >> 52) & Mask<11>::val) - 1023;

        int eatedBits = 53 - valBitCount;
        if( eatedBits < 0)
            eatedBits = 0;

        n = ((_f & 0xfffffffffffffll) | (1ll << 52)) >> eatedBits; //Keep only most significant bits

        bool s = _f >> 63;

        if( e == -1023)
            n = 0; //Denormalized number
        else if( e == (2 << 11)-1)
        {
            if( n)
                n = 0; //NaN
            else if( s)
                n = -valMask; // -Infinity
            else
                n = valMask; // +Infinity
        }
        else
        {
            if( mantissaBitCount + e - 52 + eatedBits >= 0)
                n <<= mantissaBitCount + e - 52 + eatedBits;
            else
                n >>=  -mantissaBitCount - e + 52 - eatedBits;

            if( s)
                n = -n;
        }
    }


    //Conversion from Fixed point
    template<class U> U toUnsignedInteger() const
    {
        int t = (n & valMask) >> mantissaBitCount;
        return reinterpret_cast<U&>(t);
    }

    template<class U> U toSignedInteger() const
    {
        int t = n >> mantissaBitCount;
        U u = reinterpret_cast<U&>(t);
        return u;
    }

    operator unsigned int () const
    {
        return toUnsignedInteger<unsigned int>();
    }

    operator unsigned short int () const
    {
        return toUnsignedInteger<unsigned short int>();
    }

    operator unsigned long int () const
    {
        return toUnsignedInteger<unsigned long int>();
    }

    operator unsigned long long int () const
    {
        return toUnsignedInteger<unsigned long long int>();
    }

    operator unsigned char () const
    {
        return toUnsignedInteger<unsigned char>();
    }

    operator int () const
    {
        return toSignedInteger<int>();
    }

    operator short int () const
    {
        return toSignedInteger<short int>();
    }

    operator long int () const
    {
        return toSignedInteger<long int>();
    }

    operator long long int () const
    {
        return toSignedInteger<long long int>();
    }

    operator char () const
    {
        return toSignedInteger<char>();
    }

    operator float () const
    {
        int b = 0x7fffffff;
        if( n == b+1)
            return 1.0;

        if( n == 0)
            return 0;

        int mask = n >> signeBit;
        int v = (n + mask) ^ mask;
        int pos;

        //Find last non-zero digit
        if( bitCount == 32)
        {

            v |= v >> 1; // first round down to power of 2
            v |= v >> 2;
            v |= v >> 4;
            v |= v >> 8;
            v |= v >> 16;
            v = (v >> 1) + 1;

            //Use De Bruign sequence
            pos = DeBruign5LookupTable[((unsigned long int)v * DeBruign5Seq) >> 27];
        }
        else
        {

            //Find last non-zero digit
            v |= v >> 1; // first round down to power of 2
            v |= v >> 2;
            v |= v >> 4;
            v |= v >> 8;
            v |= v >> 16;
            int i = 32; //Avoid warning
            v |= v >> i;
            v = (v >> 1) + 1;

            //Use De Bruign sequence
            pos = DeBruign6LookupTable[((unsigned long long int)v * DeBruign6Seq) >> 54];
        }

        unsigned int _n = (n + mask) ^ mask;
        if( pos > 23)
            _n >>= pos - 23;
        else
            _n <<= 23 - pos;
        _n &= Mask<23>::val;

        //Find exponent
        int e = pos - mantissaBitCount;

        int s = n < 0;

        unsigned int _f = (s << 31) | ((e+127)<<23) | _n;
        return reinterpret_cast<float&>(_f);
    }

    operator double () const
    {
        int b = 0x7fffffff;
        if( n == b+1)
            return 1.0;

        if( n == 0)
            return 0;

        int mask = n >> signeBit;
        int v = (n + mask) ^ mask;
        int pos;

        //Find last non-zero digit
        if( bitCount == 32)
        {

            v |= v >> 1; // first round down to power of 2
            v |= v >> 2;
            v |= v >> 4;
            v |= v >> 8;
            v |= v >> 16;
            v = (v >> 1) + 1;

            //Use De Bruign sequence
            pos = DeBruign5LookupTable[((unsigned long int)v * DeBruign5Seq) >> 27];
        }
        else
        {
            int v = (n + valMask) ^ valMask;

            //Find last non-zero digit
            v |= v >> 1; // first round down to power of 2
            v |= v >> 2;
            v |= v >> 4;
            v |= v >> 8;
            v |= v >> 16;
            int i = 32; //Avoid warning
            v |= v >> i;
            v = (v >> 1) + 1;

            //Use De Bruign sequence
            pos = DeBruign6LookupTable[((unsigned long long int)v * DeBruign6Seq) >> 54];
        }

        unsigned long long int _n = (n + mask) ^ mask;

        if( pos > 52)
            _n >>= pos - 52;
        else
            _n <<= 52 - pos;
        _n &= 0xfffffffffffffll;

        //Find exponent
        int e = pos - mantissaBitCount;

        int s = n < 0;

        unsigned long long int _f = ((unsigned long long int)s << 63) | ((unsigned long long int)(e+1023)<<52) | _n;
        return reinterpret_cast<double&>(_f);
    }

    //Operators


    bool operator<( const angle &_n) const
    {
        return n < _n.n;
    }

    bool operator==( const angle &_n) const
    {
        return n == _n.n;
    }

    angle & operator+=(const angle &_n)
    {
        n += _n.n;
        return *this;
    }

    angle operator-()
    {
        angle f;
        f.n = -n;
        return f;
    }

    angle & operator-=(const angle &_n)
    {
        n -= _n.n;
        return *this;
    }

    angle & operator*=(const angle &_n)
    {
        signed long long int n1 = (signed long long)n * (signed long long)_n.n;
        n = n1 >> mantissaBitCount;
        return *this;
    }

    angle & operator/=(const angle &_n)
    {
        if( _n.n == 0)
        {
            if( n < 0)
                n = -valMask; // -infinity
            else if( n > 0)
                n = valMask; // + infinity
            else
                n = 0; // error ?
            return *this;
        }

        signed long long int n2 = n;
        n2 = (n2 << mantissaBitCount) / (signed long long)_n.n;
        n = n2;

        return *this;
    }

    angle abs()
    {
        int mask = n >> signeBit;
        angle a;
        a.n = (n + mask) ^ mask;
        return a;
    }

    float toRadian()
    {
        return ((float)(*this)) * M_PI;
    }

    float toDegree()
    {
        return ((float)(*this)) * 180;
    }

    template<class Archive> void serialize(Archive & ar, const unsigned int version);
};

template<class U, int I> FixedPoint<U,I>::FixedPoint(angle a)
{
    int b = 0x7fffffff;
    b += 1;

    if( a.n == b)
    {
        *this = 1;
        return;
    }
    FixedPoint<int,31> f; f.n = a.n;
    *this = f;
}

template<class T, int I> template<class Archive> void FixedPoint<T,I>::serialize(Archive & ar, const unsigned int version)
{
    ar & boost::serialization::make_nvp("n",n);
}

template<class Archive> void angle::serialize(Archive & ar, const unsigned int version)
{
    ar & boost::serialization::make_nvp("n",n);
}

extern angle fromRadian(float r);
extern angle fromDegree(float d);

template<class T, int I> FixedPoint<T,I> sqrt(FixedPoint<T,I> n)
{
    return FixedPoint<T,I>(::sqrt((float)n));
}

template<class T, int I> FixedPoint<T,I> cos(FixedPoint<T,I> n)
{
    return FixedPoint<T,I>(::cos((float)n));
}

template<class T, int I> FixedPoint<T,I> sin(FixedPoint<T,I> n)
{
    return FixedPoint<T,I>(::sin((float)n));
}

template<class T, int I> FixedPoint<T,I> abs(FixedPoint<T,I> n)
{
    return n.abs();
}

inline angle sqrt(angle n)
{
    return angle(::sqrt((float)n));
}

inline angle cos(angle n)
{
    return angle(::cos((float)n));
}

inline angle sin(angle n)
{
    return angle(::sin((float)n));
}

inline angle abs(angle n)
{
    return n.abs();
}

template<class T, int I> std::ostream & operator<< ( std::ostream & os,const FixedPoint<T,I> & f)
{
    os << (float)f;
    return os;
}

template<class T, int I> std::istream & operator>> ( std::istream & is, FixedPoint<T,I> & f)
{
    float i;
    is >> i;
    f = i;
    return is;
}

inline std::ostream & operator<< ( std::ostream & os,angle & f)
{
    os << (float)f;
    return os;
}

inline std::istream & operator>> ( std::istream & is, angle & f)
{
    float i;
    is >> i;
    f = i;
    return is;
}

typedef FixedPoint<int, 7> fixed;

} //namespace math

#endif // FIXED_H_INCLUDED
